#ifndef DJI_CONTROLLER_H
#define DJI_CONTROLLER_H

#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <nav_msgs/Odometry.h>
#include "quad_msgs/ControlCommand.h"
#include "quad_msgs/ThrustAndAttitude.h"
#include "transform_utils/geometry_transform.h"
#include "transform_utils/type_transform.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

namespace Planner
{
    using Eigen::Quaterniond;
    using Eigen::Vector3d;
    using Eigen::Matrix3d;
    using std::string;

    class DJIController
    {
    public:
        DJIController(const ros::NodeHandle &node_handle);
        ~DJIController() = default;

        void init();
    
    private:
        ros::NodeHandle nh_;

        ros::Publisher control_command_pub_;
        ros::Publisher curr_odom_pub_;
        ros::Subscriber control_command_sub_;
        ros::Subscriber gps_position_sub_;
        ros::Subscriber gps_velocity_sub_;
        ros::Subscriber dji_acceleration_sub_;
        ros::Subscriber dji_orien_sub_;
        ros::Subscriber laser_position_sub_;

        // dji_sdk相关数据
        Vector3d gps_pos_;
        Vector3d gps_vel_;
        Vector3d dji_acc_;
        Quaterniond dji_orien_;
        bool init_gps_;
        double init_latitude_, init_longitude_;
        double init_hegiht_;
        double prev_yaw_;

        // ENU与FLU的初始变换
        Quaterniond flu2enu_;
        bool init_transform_;

        // SLAM相关数据
        Vector3d laser_pos_;
        Quaterniond laser_orien_;

        // 目标状态
        Vector3d desire_pos_, desire_vel_, desire_acc_;
        double desire_yaw_;

        // PID相关数据
        Vector3d ei_pos, ei_vel, ei_acc;
        double kp_pos, kp_vel, kp_acc;
        double ki_pos, ki_vel, ki_acc;

        // 相关常量，部分需标定后获得
        double C_EARTH;
        double gra_;
        double mass_;
        double max_thrust_;

        double tol_;
        bool gps_or_laser_;    // 选用GPS或slam信息作为里程计

        // TODO 验证dji_sdk中各话题的实际物理意义
        void controlCommandCallback(const quad_msgs::ControlCommand &msg);
        
        void gpsPositionCallback(const sensor_msgs::NavSatFix &msg);

        void gpsVelocityCallback(const geometry_msgs::Vector3Stamped &msg);

        void djiAccelerationCallback(const geometry_msgs::Vector3Stamped &msg);
    
        void djiOrienCallback(const geometry_msgs::QuaternionStamped &msg);
        
        void laserPositionCallback(const nav_msgs::Odometry &msg);

        void calcOutput();
    };
} // namespace Planner

#endif
